#include "data_struct/base/DataSet.h"
#include "modules/MonitorModule/BrushWindedMonitor.h"
#include "modules/CtrlModule/PassivePlanner/BrushWindedPlanner.h"

namespace  behavior_controller
{
    #define MAX_BRUSH_POWER 200
    #define BRUSH_SPEED 0.2
    #define MAX_CURRENT 2
    BrushWindedMonitor brush_winded_monitor;
    void BrushWindedMonitor::run()
    {
        if(brush_winded_planner.finished)
        {
            bool is_winded=false;
            if((DataSet::motor_data.power_brush_motor()>=MAX_BRUSH_POWER    &&
                DataSet::motor_data.brush_speed()>=BRUSH_SPEED)       &&
                DataSet::motor_data.overcurrent_flag()>=MAX_CURRENT)
            {
                is_winded=true;
            }
            if(is_winded)
            {
                DataSet::ctrl_state = BRUSH_WINDER_PLAN;
                brush_winded_planner.finished=false;
                std::cout<< "brush_winded  bad !"<< std::endl;
            }
        }
    }
}